#include <iostream>

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "rosbag/bag.h"
#include "rosbag/view.h"

void writeBag(const std::string &path) {
    const std::string topic = "demo8";
    rosbag::Bag bag;
    bag.open(path, rosbag::BagMode::Write);
    ros::Rate rate(1); // 频率，每隔1秒写一次
    for (uint32_t i = 0; i < 5; ++i) {
        std_msgs::String msg;
        const ros::Time &time = ros::Time::now();
        msg.data = "data: " + std::to_string(i);
        bag.write(topic, time, msg);
        ROS_INFO("write ==> time: %lf, data: %s", time.toSec(), msg.data.c_str());
        rate.sleep();
    }
    bag.close();
}

void readBag(const std::string &path) {
    rosbag::Bag bag;
    bag.open(path, rosbag::bagmode::Read);
    for (rosbag::MessageInstance const m : rosbag::View(bag)) {
        const ros::Time &time = m.getTime();
        std_msgs::String::ConstPtr msg = m.instantiate<std_msgs::String>();
        if (msg != nullptr) {
            ROS_INFO("read ==> time: %lf, data: %s", time.toSec(), msg->data.c_str());
        }
    }
}

int main(int argc, char* argv[]) {
    ros::init(argc, argv, "demo8");
    ros::NodeHandle nodeHandle;
    std::string path = "./build/test.bag";
    writeBag(path);
    readBag(path);
    system(std::string("rm -rf " + path).c_str());

    return EXIT_SUCCESS;
}